#include "Serial.h"
#include <fstream>
#include "serialdriver.h"

using namespace std;

serialdriver::serialdriver()
{
    //初始化相关变量
    _iscalibrated = false;

    _mserial = new SerialHandler();

    _mserial->init();

    _calibrate_cnt = 0;

    _processeddata = new double[6];

    _xbias = 0, _ybais = 0, _zbias = 0;

    _readok = false;

}

serialdriver::~serialdriver()
{
    delete _processeddata;
}

bool serialdriver::readdata()
{
    _mserial->Read();

    if(_mserial->isReadOK == 1)
    {
        _mserial->isReadOK = 0;	

        if(processdata(_mserial->readbuff, _processeddata) == true)
        {
            _readok = true;

            //标定零漂
            if(!_iscalibrated)
            {
                _calibrate_cnt ++;

                _xbias = _xbias + _processeddata[0];
                _ybais = _ybais + _processeddata[1];
                _zbias = _zbias + _processeddata[2];	

                if(_calibrate_cnt == calibrate_cnt_total)
                {
                    _xbias = _xbias / calibrate_cnt_total;
                    _ybais = _ybais / calibrate_cnt_total;
                    _zbias = _zbias / calibrate_cnt_total;
                    _iscalibrated = true;
                }
            }

            _processeddata[0] = _processeddata[0] - _xbias;
            _processeddata[1] = _processeddata[1] - _ybais;
            _processeddata[2] = _processeddata[2] - _zbias;
        }
        return true;
    }
    _readok = false;
    return false;
}

bool serialdriver::processdata(uint8 * dataPacket, double * databuff)
{
    float m1 = 0.0,m2 = 0.0, m3 = 0.0;
    //

    if(dataPacket[0] == 0x55 && dataPacket[1] == 0xaa)
    {
        //检验校验和
        unsigned int sum = 0;
        for(int i = 2; i < PKGSIZE - 1; i++)
        {
            sum += dataPacket[i];
        }

        //std::cout<< sum  << " " << imumsg[14] << std::endl;
        //
        if( sum % 256 == dataPacket[PKGSIZE - 1])
        {
            //解算加速度X,Y,Z
            int16 AccX = Byte16(int16, dataPacket[2], dataPacket[3]);
            int16 AccY = Byte16(int16, dataPacket[4], dataPacket[5]);
            int16 AccZ = Byte16(int16, dataPacket[6], dataPacket[7]);
            float Accscalar = MPU9250A_4g * GRAVITY_MSS;
            float AccTrueX = AccX * Accscalar;
            float AccTrueY = AccY * Accscalar;
            float AccTrueZ = AccZ * Accscalar;

            //解算角速度X,Y,Z
            int16 GyrX = Byte16(int16, dataPacket[8], dataPacket[9]);
            int16 GyrY = Byte16(int16, dataPacket[10], dataPacket[11]);
            int16 GyrZ = Byte16(int16, dataPacket[12], dataPacket[13]);
            float Gyrscalar = UserGyrResolution / 57.3;
            float GyrTrueX = GyrX * Gyrscalar;
            float GyrTrueY = GyrY * Gyrscalar;
            float GyrTrueZ = GyrZ * Gyrscalar;

            //解算Mag X,Y,Z
            int16 MagX = Byte16(int16, dataPacket[15], dataPacket[14]);
            int16 MagY = Byte16(int16, dataPacket[17], dataPacket[16]);
            int16 MagZ = Byte16(int16, dataPacket[19], dataPacket[18]);

            float Magscalar = MPU9250M_4800uT*Tesla2Gauss;
            float magx = MagX*Magscalar;
            float magy = MagY*Magscalar;
            float magz = MagZ*Magscalar;

            //发送Imu数据
            databuff[0] = GyrTrueX;
            databuff[1] = GyrTrueY;
            databuff[2] = GyrTrueZ;

            databuff[3] = AccTrueX;
            databuff[4] = AccTrueY;
            databuff[5] = AccTrueZ;

            return true;
        }
        else
        {
            cout << "Checksum Error" << endl;
            return false;
        }
    }
    else
    {
        cout << "Header Error" << endl;
        return false;
    }
}

